#include "Header.h"

void GrayTrack_Init(void)
{
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);

    GPIO_InitTypeDef GPIO_InitStructure;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;
    GPIO_InitStructure.GPIO_Pin = OUT3 | OUT4 | OUT5 | OUT6 | OUT7 | OUT8;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_Init(GPIOA, &GPIO_InitStructure);

    GPIO_InitStructure.GPIO_Pin = OUT1 | OUT2;
    GPIO_Init(GPIOB, &GPIO_InitStructure);
}

/*灰度传感器循迹（小角度弯道开环控制，直角转弯闭环控制）*/
void GrayTrack_Track(void)
{
    if((D1 == 1)&&(D2 == 0)&&(D3 == 0)&&(D4 == 0)&&(D5 == 0)&&(D6 == 0)&&(D7 == 0)&&(D8 == 0))        Motor_SetSpeed(0,50);      //0000 0001
    else if((D1 == 1)&&(D2 == 1)&&(D3 == 0)&&(D4 == 0)&&(D5 == 0)&&(D6 == 0)&&(D7 == 0)&&(D8 == 0))   Motor_SetSpeed(10,50);     //0000 0011
    else if((D1 == 0)&&(D2 == 1)&&(D3 == 0)&&(D4 == 0)&&(D5 == 0)&&(D6 == 0)&&(D7 == 0)&&(D8 == 0))   Motor_SetSpeed(15,40);     //0000 0010
    else if((D1 == 0)&&(D2 == 1)&&(D3 == 1)&&(D4 == 0)&&(D5 == 0)&&(D6 == 0)&&(D7 == 0)&&(D8 == 0))   Motor_SetSpeed(20,40);     //0000 0110
    else if((D1 == 0)&&(D2 == 0)&&(D3 == 1)&&(D4 == 0)&&(D5 == 0)&&(D6 == 0)&&(D7 == 0)&&(D8 == 0))   Motor_SetSpeed(25,40);     //0000 0100
    else if((D1 == 0)&&(D2 == 0)&&(D3 == 1)&&(D4 == 1)&&(D5 == 0)&&(D6 == 0)&&(D7 == 0)&&(D8 == 0))   Motor_SetSpeed(35,40);     //0000 1100
    else if((D1 == 0)&&(D2 == 0)&&(D3 == 0)&&(D4 == 1)&&(D5 == 0)&&(D6 == 0)&&(D7 == 0)&&(D8 == 0))   Motor_SetSpeed(35,40);     //0000 1000
    else if((D1 == 0)&&(D2 == 0)&&(D3 == 0)&&(D4 == 1)&&(D5 == 1)&&(D6 == 0)&&(D7 == 0)&&(D8 == 0))   Motor_SetSpeed(40,40);     //0001 1000 //正中间位置
    else if((D1 == 0)&&(D2 == 0)&&(D3 == 0)&&(D4 == 0)&&(D5 == 1)&&(D6 == 0)&&(D7 == 0)&&(D8 == 0))   Motor_SetSpeed(40,35);     //0001 0000
    else if((D1 == 0)&&(D2 == 0)&&(D3 == 0)&&(D4 == 0)&&(D5 == 1)&&(D6 == 1)&&(D7 == 0)&&(D8 == 0))   Motor_SetSpeed(40,35);     //0011 0000
    else if((D1 == 0)&&(D2 == 0)&&(D3 == 0)&&(D4 == 0)&&(D5 == 0)&&(D6 == 1)&&(D7 == 0)&&(D8 == 0))   Motor_SetSpeed(40,25);     //0010 0000
    else if((D1 == 0)&&(D2 == 0)&&(D3 == 0)&&(D4 == 0)&&(D5 == 0)&&(D6 == 1)&&(D7 == 1)&&(D8 == 0))   Motor_SetSpeed(40,20);     //0110 0000
    else if((D1 == 0)&&(D2 == 0)&&(D3 == 0)&&(D4 == 0)&&(D5 == 0)&&(D6 == 0)&&(D7 == 1)&&(D8 == 0))   Motor_SetSpeed(40,15);     //0100 0000
    else if((D1 == 0)&&(D2 == 0)&&(D3 == 0)&&(D4 == 0)&&(D5 == 0)&&(D6 == 0)&&(D7 == 1)&&(D8 == 1))   Motor_SetSpeed(50,10);     //1100 0000
    else if((D1 == 0)&&(D2 == 0)&&(D3 == 0)&&(D4 == 0)&&(D5 == 0)&&(D6 == 0)&&(D7 == 0)&&(D8 == 1))   Motor_SetSpeed(50,0);      //1000 0000
    else if((D1 == 1)&&(D2 == 1)&&(D3 == 1)/* &&(D4 == 0)&&(D5 == 0) */&&(D6 == 0)&&(D7 == 0)&&(D8 == 0)) //111x 0000
    {
        Motor_Turn90(Left);
    }
    else if((D1 == 0)&&(D2 == 0)&&(D3 == 0)/* &&(D4 == 0)&&(D5 == 0) */&&(D6 == 1)&&(D7 == 1)&&(D8 == 1)){
        Motor_Turn90(Right);
    }
    else  Motor_SetSpeed(20,20);
}
